#ifndef INTERFACE_HEADER_H
#define INTERFACE_HEADER_H

#include <stdlib.h>
#include <string>
#include "JointGroupCtrlClient.hpp"
#include "ImageClient.hpp"
#include <vector>

// All function calls are blocking

namespace MarsNamespace
{
  class MarsManipulator {
    public:
      enum ECommandResult { SUCCESS, FAILURE, UNKNOWN };
      MarsManipulator(  std::string,  int, std::string , std::string );

      ~MarsManipulator();

      // Picking up objects

      //moves manipulator to object_x[mm] object_y[mm] from manipulator origin
      ECommandResult pickObject(double object_x, double object_y);
      //with this you force the orientation of the gripper to not change
      ECommandResult movewithconstCto(double atx,double aty);
      //give pixel from cam image and then does as pickObject
      ECommandResult movetoPosfromI(float clickedy, float clickedx);


      // Storing and delivering objects pocket 0 is left, pocket 1 is right and then back to default
      //works from ground position
      ECommandResult storeObjectToPocket(int pocket);
      //NOTE unused
      ECommandResult deliverFromPocket(int pocket);
      //basically same as release
      ECommandResult deliverFromManipulator();
      //moves manipulator down and grabs
      ECommandResult grabObject();
      //move all down
      ECommandResult toBottom(double scale = 1.0);
      //move all up
      ECommandResult toTop();
      //close gripper
      ECommandResult grab();
      //open gripper
      ECommandResult release();

      //extends arm full shoulder, elbow and wrist at angle 0
      ECommandResult extendArm();
      //moves arm to travelling position (right now that is shoulder -90, elbow = wrist = 90) or mirrored position
      ECommandResult retractArm(int pocket);
      // Manipulator control
      ECommandResult openManipulator();
      //unimplemented
      ECommandResult closeManipulator();

      //used in all functions to block calls and get an answer
      ECommandResult isCommandExecuted();

      //get
      MaCI::JointGroupCtrl::TJointPositionArray getManipulatorPosition();

			//joint movement
			//*****************************************************************************
			ECommandResult moveShoulder(int angle);
			ECommandResult moveElbow(int angle);
			ECommandResult moveWrist(int angle);
			ECommandResult moveGripper(int angle);
			//*****************************************************************************
			//incremental
			ECommandResult moveStepX();
			ECommandResult movenegStepX();
			ECommandResult moveStepY();
			ECommandResult movenegStepY();
			//*****************************************************************************
			ECommandResult resetMicro();
			ECommandResult changeSpeed(int speed);
			//*****************************************************************************
			ECommandResult specifyCorners(std::vector<float> corners);
  };

  class MarsCamera {

    public:

      MarsCamera(std::string gimnetap,  int port, std::string name, std::string source);
      ~MarsCamera() {}

      //hand over image
      MaCI::Image::CImageContainer getImage();

    private:
      //TODO: hide these away from interface_header!
      void init();
      const MaCI::Image::CImageClient ic;

  };
}

#endif // INTERFACE_HEADER_H_INCLUDED
